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Abstract 


Position  determination  for  a  mobile  robot  is  an  important  part  of  autonomous  navigation.  In  many  cases,  dead 
reckoning  is  insufficient  because  it  leads  to  large  inaccuracies  over  time.  Beacon-  and  landmark-based  estimators 
require  the  emplacement  of  beacons  and  the  presence  of  natural  or  man-made  structure  respectively  in  the 
environment.  In  this  paper  we  present  a  new  algorithm  for  efficiently  computing  accurate  position  estimates  based  on 
a  radially-scanning  laser  rangefinder  that  requires  minimal  structure  in  the  en\  uonment.  The  algorithm  employs  a 
connected  set  of  short  line  segments  to  approximate  the  shape  of  any  environment  and  can  easily  be  consuucted  by 
the  rangefinder  itself.  We  describe  techniques  for  efficiently  managing  the  environment  map,  matching  the  sensor 
data  to  the  map,  and  computing  the  robot’s  position.  We  present  accuracy  and  runtime  results  for  our  implementation. 
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1.0  introduction 


Determining  the  location  of  a  robot  relative  to  an  absolute  coordinate  frame  is  one  of  the  most  important  issues  in  the 
autonomous  navigation  problem.  In  a  two  dimensional  space,  the  location  of  a  mobile  robot  can  be  represented  by  a 
triplet  (tjj,  ty,  9)  known  as  the  robot  pose.  A  mobile  coordinate  system  (Robot  Frame)  attached  to  the  robot  can  be  con¬ 
sidered  such  that  (Lj,  ty)  represents  the  translation  (position)  of  the  Robot  Frame  with  respect  to  an  absolute  coordi¬ 
nate  system  (World  Frame)  and  6  represents  its  orientation  (heading)  (Figure  1). 


FIGURE  1 .  World  Frame  and  Robot  Frame 


The  simplest  approach  in  estimating  the  robot  pose  (position  and  orientation)  is  provided  by  the  dead  reckoning  sys¬ 
tem,  in  which  both  position  and  orientation  are  given  by  counting  the  revolutions  of  the  wheels  [17],  The  advantage 
of  dead  reckoning  is  that  it  is  simple  and  inexpensive.  However,  assuming  even  very  small  errors,  as  the  robot  moves 
from  one  place  to  another,  the  uncertainty  about  its  pose  grows. 

In  order  to  reduce  this  uncertainty  registration  with  the  environment  is  required.  Different  approaches  have  been  pro¬ 
posed.  A  significant  number  use  landmark  recognition,  either  extracting  relevant  natural  features  (comers,  objects, 
etc.)  using  a  camera  [8, 12]  or  identifying  prelocated  beacons  using  both  a  camera  [9]  and  an  optical  scanner  [20]. 
The  major  disadvantages  of  the  first  are  the  poor  accuracy  as  well  as  the  need  for  a  suitable  environment.  Major  draw¬ 
backs  of  beacon-based  navigation  are  that  the  beacons  must  be  placed  within  range  and  must  be  appropriately  config¬ 
ured  in  the  robot  work  space. 

An  alternate  approach  consists  of  the  comparison  of  dense  range  data  to  model  data.  Since  both  accuracy  and  robust¬ 
ness  in  a  position  estimator  depend  on  the  amount  of  data  used  in  the  comparison,  this  approach  presents  some  impor¬ 
tant  advantages  over  the  two  above  mentioned. 

The  matching  problem  between  two  sets  of  data  can  be  formulated  in  two  different  ways:  feature-based  and  iconic.  In 
the  feature-based  method,  a  set  of  features  is  extracted  from  the  sensed  data  (such  as  line  segments,  comers,  etc.)  and 
then  matched  against  the  corresponding  features  in  the  model.  Shaffer  et  al.  [1]  using  a  laser  scanner  rangefinder  and 
Crowley  [5]  and  Drumheller  [14]  using  range  from  a  rotating  sonar,  proposed  a  feature-based  approach  for  a  2-D 
environment.  Krotkov  [11]  used  a  single  CCD  camera  to  extract  angles  to  vertical  edges  within  the  scene  and  then 
determine  the  robot  pose  by  establishing  their  correspondence  in  a  2-D  map. 

In  contrast,  the  iconic  method  works  directly  on  the  raw  sensed  data,  minimizing  the  discrepancy  between  it  and  the 
model.  Hebert  et  al.  [13]  formulated  an  iconic  method  to  compare  two  elevation  maps  acquired  from  a  3-D  laser 
rangefinder.  Moravec  and  Elfes  [4, 18]  proposed  a  technique  to  match  two  maps  represented  by  occupancy ^rids.  The 
occupancy  grid  models  the  environment  using  a  spatial  grid  of  cells,  where  each  cell  has  a  probability  value  assigned 
to  it  representing  the  likelihood  that  the  cell  is  occupied.  Finally,  Cox  [2]  used  an  infrared  laser  rangefinder  to  get  a  2- 
D  radial  representation  of  the  environment  which  is  matched  against  a  line  segment  map. 
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In  this  paper,  we  present  a  new  iconic  approach  for  estimating  the  pose  of  a  mobile  robot  equipped  with  a  radial  laser 
rangefinder.  Unlike  prior  approaches,  our  method  can  be  used  in  environments  with  only  a  minimal  amount  of  struc¬ 
ture,  provided  enough  is  present  to  disambiguate  the  robot’s  pose.  Our  map  consists  of  a  possibly  large  number  of 
short  line  segments,  perhaps  constructed  by  the  rangefinder  itself,  to  approximately  represent  any  environment  shape. 
This  representation  introduces  problems  in  map  indexing  and  error  minimization  which  are  addressed  to  insure  that 
accurate  estimates  can  be  computed  quickly. 


2.0  Problem  statement 


The  position  estimation  problem  consists  of  two  parts:  sensor  to  map  data  correspondence  and  error  minimization. 
Correspondence  is  the  task  of  determining  which  map  data  point  gave  rise  to  each  sensor  data  point.  Once  the  corre¬ 
spondence  is  computed,  error  minimization  is  the  task  of  computing  a  robot  pose  that  minimizes  the  error  (e.g.,  dis¬ 
tance)  between  the  actual  location  of  each  map  data  point  and  the  sensor’s  estimate  of  its  location.  In  this  section  we 
formalize  these  two  parts. 

Let  A  be  a  set  of  sensed  data  points  taken  from  a  certain  robot  position,  and  B  be  a  set  of  either  sensed  data  extracted 
from  a  different  position  or  a  set  of  data  describing  a  world  map.  The  matching  problem  for  position  estimation  can 
be  stated  as  how  to  determine  the  best  mapping  of  A  onto  B,  in  other  words,  how  to  compute  the  transformation  T 
which  minimizes  an  appropriate  error  function: 

E  =  0(T(A),B)  (EQ  1) 

Consider  a  world  map  M  described  by  a  set  of  data  M  =  (Mj,  M2, ...  .M^)  expressed  in  an  absolute  coordinate  sys¬ 
tem  (World  Frame)  and  sensed  data  S  =  {Sj,  S2, ...  ,$„),  where  Sj  is  expressed  in  a  local  coordinate  system  attached  to 
the  mobile  robot  (Robot  Frame).  Then,  the  robot  position  estimation  consists  of  solving  the  minimization  problem: 

min-j.  E  =  0(7(5),^)  (EQ  2) 

In  this  paper  we  are  concerned  with  scanned  data  taken  from  two-dimensional  world  maps.  A  convenient  way  to 
describe  these  maps  is  by  means  of  a  set  M  =  L=  {Lj,  L2>  —  J-m)  where  Lj  represents  the  line  segment  between  the 
“left”  point  (aj*,  bj*)  and  the  “right”  point  (aj^  bjO  in  the  World  Frame  (see  Figure  2).  This  line  segment  lies  on  the  line 
given  in  an  implicit  normalized  form  by: 

AjX  +  BjY+Cj  =  0  3) 


FIGURE  2  .  Different  distances  to  consider  for  each  line  segment 


In  the  iconic  approach  presented  in  this  paper,  the  sensed  data  consists  of  points  taken  from  a  radial  laser  scanner. 
These  points  are  referred  to  as  either  sensed  points  or  scanned  points. 
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Now,  let  3?  be  the  binary  relation  between  the  finite  sets  S  and  L 

’3{<zSxL  (EQ  4) 

This  relation  can  be  represented  by  means  of  the  nxm  correspondence  matrix  R  with  elements  given  by: 
rjj  =  1  if  the  point  S,  is  a  measurement  of  segment  Lj 
rjj  =  0  otherwise 

Determining  this  matrix  R  is  equivalent  to  solving  the  correspondence  problem  formulated  as  determining  which  line 
segment  Lj  from  the  model  L  gave  rise  to  the  image  point  =  Pi=  (Xj.yj)^.  Obviously,  because  one  point  can  corre¬ 
spond  to  only  one  line  segment,  each  of  the  rows  of  R  will  contain  just  one  value  “  1 

A  reasonable  criterion  for  constructing  the  matrix  R  is  Uie  minimum  Euclidean  distance  (this  will  be  emphasized  in 
the  next  section).  Thus,  let  D  be  the  nxm  distance  matrix  whose  elements  dij  are  the  distances  between  the  sensed 
point  Pi  =  (Xi,Yi)^  and  the  line  segment  Lj  defined  as  follows: 

dij  =  d°  if  (a°j,b°j)  is  an  ele  -nent  of  Lj 

dij  =  min(d^  d*)  otherwise 

where  (see  Figure  2): 


where  (aJ  +  bJ)  =  1 

2  2  ^ 
4  =  {(X.-a))  +  (T.  -b;)  ) 

2  2 
4  =  ax -a])  ) 


cos0  -sin0 

I', 

= 

sin0  COS0  ty 

1 

0  0  1 

1 

(EQ  5) 

(EQ  6) 

(EQ  7) 

(EQ  8) 


Equation  8  defines  the  transformation  between  a  point  Pi  in  the  World  Frame  and  a  point  Pi  in  the  Robot  Frame  given 
by  (tj,ty,0)  (see  Figure  1). 


From  the  matrices  D  and  R  it  is  possible  to  obtain  the  error  vector  between  the  sensed  points  and  the  corresponding 
line  segments: 


e  =  {e^...e^a  =  diag{DxR‘) 


(EQ  9) 


It  should  be  noted  that  e  is  a  function  of  the  transformation  defined  in  Equation  8.  Then,  the  parameter  to  minimize 
can  be  expressed  as: 


/  =  ^e,(f„r^,e)  (EQ  10) 

1  =  1 


The  iconic  position  estimation  problem  consists  of  the  computation  of  (^,ty,0)  to  solve: 
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In  practice,  the  matrix  R  is  derived  from  D  by: 
rjjj  =  1  if  dji^  <  djj  for  all  j 
r,).  =  0  otherwise 

Moreover,  R  is  a  “dynamic  matrix”  in  the  sense  that  it  is  updated  after  each  iteration  of  the  optimization  algorithm  to 
solve  Equation  11.  A  similar  approach  was  used  by  Cox  [2]. 

Although  the  approach  presented  here  to  compute  R  is  deterministic,  this  formulation  enables  one  to  treat  the  corre¬ 
spondence  problem  in  a  probabilistic  way.  In  this  formulation,  the  elements  rjj  of  the  matrix  R  would  no  longer  be 
either  1  or  0  but  would  express  the  probability  that  the  point  Pj  corresponds  to  the  segment  line  Lj. 

One  of  the  main  issues  when  solving  the  matching  problem  is  the  high  cost  involved  in  computing  the  nxm  matrix  D. 
In  Section  3  we  present  a  more  efficient  approach  to  determine  DxR^  avoiding  the  entire  computation  of  the  matrix 
D. 


3.0  Iconic  Position  Estimation 


In  this  section,  the  two  subproblems  (correspondence  and  minimization)  of  the  iconic  position  estimation  problem  are 
described.  Because  the  world  map  is  represented  in  an  absolute  coordinate  system  (World  Frame)  and  the  sensed  data 
is  represented  in  a  mobile  coordinate  system  attached  to  the  robot  (Robot  Frame)  a  common  representation  for  both 
must  be  considered.  This  can  be  accomplished  by  using  Equation  8  with  a  first  estimate  of  the  robot  pose  (t^,  ty,  0) 
provided  by  the  dead  reckoning  system  (Figure  4). 

Once  the  line  segments  from  the  map  and  scanned  points  are  represented  in  the  same  coordinate  system,  it  is  possible 
to  search  for  the  segment/point  correspondence  pairs. 


FIGURE  3 .  Two  levels  of  resolution  in  the  map 


To  establish  such  a  correspondence  all  of  the  segments  firom  the  map  could  be  checked  against  every  range  point  in 
order  to  compute  the  matrix  D  (O(nxm)).  In  a  sensor  such  as  Cyclone  Range  Finder,  which  will  be  described  later,  one 
thousand  scanned  points  would  have  to  be  matched  against  a  model  of  hundreds  of  line  segments  (which  could  be 
built  by  the  robot  itself).  To  avoid  this  extremely  expensive  procedure,  we  propose  a  two-tier  map  representation 
(Figure  3): 

1.-  CtUmap.  array  of  grid  cells  in  which  every  cell  is  labeled  either  occupied,  if  it  contains  at  least  one  line  segment,  or 
empty,  if  it  contains  no  segments  (see  Figure  3).  Elfes  and  Moravec  used  a  similar  approach  for  sonar  navigation  in 
their  occvpancygridlA,  18]. 
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2.- Lint.  map.  collection  of  segments  inside  each  of  the  occupied  cells  considered  for  correspondence. 

Thus,  the  correspondence  of  sensed  points  to  the  model  segments  is  accomplished  in  two  steps.  First,  a  set  of  cells  is 
selected  for  each  of  the  scanned  points.  Second,  only  those  segments  inside  these  cells  are  checked  for  correspon¬ 
dence  (Figure  4).  By  using  this  representation,  the  number  of  segments  to  be  checked  decreases  considerably,  drasti¬ 
cally  reducing  the  matching  time. 

The  grid  size  must  be  selected  according  to  the  characteristics  of  the  particular  application.  One  cell  for  the  whole 
map  is  inefficient  because  it  requires  all  of  the  line  segments  to  be  examined  for  each  sensed  point  (no  improvement 
at  all).  A  very  large  number  of  cells  is  also  inefficient  because  it  requires  a  large  number  of  empty  cells  to  be  checked. 
We  have  determined  that  the  appropriate  size  of  the  grid  is  a  function  of  a  variety  of  parameters  (number  of  line  seg¬ 
ments  in  the  model,  type  of  environment,  initial  error,  etc.)  and  therefore  an  empirical  method  is  proposed  for  choos¬ 
ing  it. 


F I  CURE  4  .  Matcher  structure 


3.1  Hierarchical  iconic  matching 

In  this  section,  we  formulate  iconic  matching  for  the  two-tier  map.  Considering  that  candidate  cells  must  be  selected 
within  which  to  search  for  the  corresponding  line  segment,  let  U  be  a  binary  nxc  matrix  defined  as  follows: 

Uiic  =  1  if  cell  k  is  selected  for  the  scanned  point  i 

Uj|5  =  0  otherwise 

Let  V  be  a  binary  c^m  matrix  defined  as  follows: 

V|jj  =  1  if  the  Ij  segment  lies  in  cell  k  (  /y  c*  ^  0  ) 
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vjjj  =  0  otherwise 

Consider  the  nxm  matrix  W  given  by  the  product  UxV.  The  elements  w’^j  are  given  by: 


*  =  I 

Consider  the  matrix  W  defined  as  follows: 

Wjj  =  1  if  w’ij  >  0 
Wy  =  0  if  w’ij  =  0 

Thus,  a  generic  element  Wy  from  the  matrix  W  indicates  whether  the  segment  Ij  lies  in  any  of  the  cells  selected  for  the 
point  P;  or  not,  and  therefore  if  the  distance  between  Ij  and  must  be  computed  or  not. 

For  a  given  point  Pj,  assuming  both  U  and  V  have  been  obtained,  the  number  of  distances  to  be  computed  reduces 
from  m  to  m’j,  where: 


m]  =  '^w.j  (EQ  12) 

;  =  i 

Although  the  matrix  U  must  be  calculated  for  each  computed  estimate,  the  matrix  V  is  computed  just  once,  when  the 
map  is  built. 

Notice  that  in  the  extreme  case  given  above  in  which  the  number  of  cells  considered  for  the  u[[  map  (c)  is  very  high  (c 
»  m),  the  computation  of  matrix  U  (nxc)  could  become  more  expensive  that  the  straightforward  approach  of  comput¬ 
ing  matrix  D  (O(nxm)).  In  the  following  section  the  methodology  to  obtain  the  matrix  U  is  presented. 

3.2  Cell  selection 

After  the  scanned  points  have  been  transformed  to  the  World  Frame,  a  set  of  occupied  cells  must  be  selected  for  each 
of  them  (Figure  4).  Due  to  errors  in  both  dead  reckoning  and  the  sensor,  in  a  significant  number  of  cases,  the  points  P; 
are  located  in  empty  cells.  We  analyze  these  errors  in  more  detail  below. 

3.2.1  Dead  Reckoning  errors 

Dead  reckoning  is  intrinsically  vulnerable  to  bad  calibration,  imperfect  wheel  contact,  upsetting  events,  etc.  Thus,  a 
bounded  confidence  region  in  which  the  actual  location  is  likely  to  be  found  is  used.  This  region  is  assumed  to  be  a 
circle  of  radius  8^  proportional  to  the  traversed  distance.  This  uncertainty  in  the  robot  position  propagates  in  such  a 
way  that  an  identical  uncertainty  region  centered  at  the  sensed  point  can  be  considered  (Figure  5a). 

In  a  similar  way,  the  heading  error  is  assumed  to  be  bounded  by  degrees.  This  error  is  also  considered  to  be  pro¬ 
portional  to  the  traversed  distance.  Notice  that  the  effect  of  this  error  over  the  uncertainty  region  depends  on  the 
range-the  bigger  the  range  the  larger  the  uncertainty  region  (Figure  5b). 
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Uncertainty  in  scanned  point 


FIGURE  5  .  Uncertainties  in  the  sensed  data  due  to  dead  reckoning  error  pose  (A)  Uncertainty  region  caused  by  the 
position  error  (B)  Dead  reckonino  position  an  orientation  error  simultaneously 


3.2.2  Sensor  errors 

Sensor  errors  arise  for  the  following  reasons: 

-  The  range  provided  by  the  laser  rangefinder  is  noisy  as  well  as  truncated  by  the  resolution  of  the  sensor. 

-  The  angular  po.«’Hon  eiven  by  the  decoder  has  some  inaccuracy. 

Thus,  the  two  errors  to  be  considered  are  rmgt  errcrand  omntatim  error.  Although  they  can  be  modeled  as  a  Gaussian 
distribution,  both  of  them  are  modeled  here  as  bounded  errors,  as  were  dead  reckoning  errors.  Their  maximum  and 
minimum  values  define  a  new  region  of  uncertainty  to  be  added  to  the  one  arising  from  the  dead  reckoning  enors. 
Figure  6a  shows  a  region  defined  by  two  errors  parameters  85  and  CjWhose  values  were  obtained  from  the  sensor  cali¬ 
bration  experiments  [15].  This  region  does  not  increase  with  the  distance  traversed  by  the  robot.  On  the  other  hand, 
although  it  depends  on  the  ran^e  value,  it  is  not  as  significant  as  the  dead  reckoning  error  (£5  «  Ej.). 


Figure  6b  shows  the  final  region  after  considering  both  dead  reckoning  and  sensor  errors.  Notice  that  the  sensed  point 
location  is  not  necessarily  along  the  scanning  ray  but  inside  an  uncertainty  region. 


FIGURE 


6  .  (a)  Uncertainly  in  the  sensed  data  due  to  the  sensor  errors  (b)  Uncertainty  regio'  caused  by  dead 
reckoning  and  sensor  errors 
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3.2.3  Cells  Selection  algorithm 

The  algorithm  to  select  the  cells  takes  into  account  the  above  mentioned  uncertainty  regions.  Each  time  the  cell  which 
includes  the  scanned  point  is  labeled  empty  a  search  for  a  nearby  occupied  cell  is  performed  (Figure  7).  The  searching 
area  is  selected  to  be  coincident  with  the  uncertainty  region  given  by  the  sensor  and  dead  reckoning  errors  (Figure 
6b). 

If  no  a  priori  information  is  available,  the  matcher  assumes  the  closest  occupied  cells  are  the  best  ones  in  terms  of 
confidence.  A  distance  function  based  on  8-connectivity  has  been  used.  The  search  radiates  out  firom  the  cell  contain¬ 
ing  the  sensed  point  until  the  cell  containing  the  nearest  line  segment  is  found.  For  all  the  cells  located  at  the  same 
distance,  only  those  both  occupied  and  inside  the  uncertainty  region  are  examined  for  the  closest  line  segment  within 
them  (Figure  7). 


UNCERTAINTY 

REGION 


□  empty  cell 

II  occupied  cell  but  not  selected 
n  :  selected  cell 


FIGURE  7  .  Cells  to  be  considered  when  the  original  cell  is  empty 


There  is  no  guarantee  that  every  time  the  scanned  point  lies  in  an  occupied  cell,  the  closest  line  segment  is  within  it. 
Figure  8b  shows  a  simple  example  of  this  case.  The  scanned  point  should  be  assigned  to  the  segment  labeled  “Ij” 
rather  than  segment  “1^”.  The  matcher  rectifies  this  problem  by  including  the  neighbor  occupied  cells  as  candidates  to 
search  for  the  nearest  line  segment 

The  location  of  previous  scanned  points  with  respect  to  their  respective  target  line  segments  could  be  used  to  decide 
which  cells  are  more  likely  to  contain  the  target  line  segment.  This  approach  has  not  been  implemented  so  far,  but  it 
could  have  a  considerable  impact  on  the  efficiency  of  the  algorithm. 

To  make  the  algorithm  robust  against  outliers,  incompleteness  of  the  model,  presence  of  extraneous  objects,  etc.,  a 
progression  of  increasingly  better  position  estimates  is  computed  (see  Section  3.4).  The  uncertainty  region  is  reduced 
along  the  progression.  This  approach  is  based  on  the  fact  that  the  uncertainty  due  to  the  error  in  sensor  location 
decreases  as  the  position  estimate  improves.  However,  the  uncertainty  region  due  to  the  sensor  errors  does  not  vary. 
In  practice,  this  is  accomplished  by  weighting  the  parameters  5^  and  between  0  and  1. 
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FIGURE 


8  .  (a)  Line  segments  considered  for  a  particular  cell  when  the  original  cell  is  occupied  (b)  Wrong 
assignment  of  the  segment  to  the  scanned  point 


3.3  Segment  correspondence 

To  determine  which  line  segment  inside  the  assigned  cells  to  match  to  the  scanned  point,  a  minimum  distance  crite¬ 
rion  is  used  (Figure  8a).  This  assumption  is  valid  as  long  as  the  displacement  between  sensed  data  and  model  is  small 
enough.  This  assumption  limits  the  robot  traversed  distance  between  consecutive  position  estimates.  However,  since 
after  each  iteration  the  point/line-segment  pairs  are  updated,  the  limitation  can  be  relaxed  somewhat  (Figure  9). 

Given  a  scanned  point  ?,=  (Xj,  Yj),  three  different  distances  for  each  line  segment  are  computed  (Figure  2).  They  are 
given  by  Equations  5, 6  and  7.  The  smallest  distance  to  the  line  segments  inside  the  selected  cells  determines  which 
line  segment  Ij  is  to  be  matched  to  P,. 


FIGURE  9 .  Block  diagram  of  the  iconic  position  estimator 


3.4  Minimization 

After  the  matched  pairs  have  been  computed,  the  estimate  is  computed  by  minimizing  the  following; 
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min  {e^e)  =  min 


(EQ  13) 


where  ei=ei(tx,  ty,  9)  is  given  in  Section  2  by  Equation  11. 

Although  the  rotation  0  makes  this  optimization  problem  non-linear,  a  closed-form  solution  exists.  The  Schonemann 
approach  treats  the  rotation  elements  as  unknowns  and  applies  Lagrange  multipliers  to  force  the  rotation  matrix  to  be 
orthogonal  [16].  However,  we  have  opted  for  a  iterative  algorithm  (Gauss-Newton)  to  support  the  future  modelling  of 
Gaussian  uncertainty  in  the  sensor  and  robot  data.  Such  modelling  requires  nonscalar  weights  on  the  error.  No  closed- 
form  solution  is  known  to  exist  for  the  minimization.  In  this  method  the  equation  to  be  solved  is: 

e  +  J  d  =  0  (EQ  14) 

where  e  is  the  error  vector  given  by  Equation  10,  d  is  the  difference  vector  between  the  transformation  parameters  on 
successive  iterations,  and  J  is  the  Jacobian: 


^ 

^  ^ 
dt^  a/y  ae 


(EQ  15) 


Notice  that  Equation  14  is  overdetermined  for  n>3.  Then  we  use  the  pseudoinverse  of  the  Jacobian  to  find  a  least 
square  fit  of  d: 


d  =  (EQ  16) 

Equation  16  is  solved  iteratively  for  the  displacement  vector  d  until  the  absolute  value  of  its  elements  is  less  than 
some  tolerance.  On  each  iteration,  the  correspondence  between  sensor  and  model  data  is  recomputed  to  reduce  the 
effects  of  outliers  and  mismatches.  We  have  empirically  determined  that  iterating  more  than  once  between  correspon¬ 
dence  updates  yields  no  additional  accuracy  to  the  final  estimate,  thus  our  approach  is  functionally  equivalent  to  the 
closed-form  solution  with  updating. 

This  method  has  a  quadratic  convergence  and  is  less  sensitive  than  the  conventional  Newton  method  to  the  starting 
point  and  the  conditioning  of  the  Jacobian.  However,  for  this  algorithm  to  converge  to  a  correct  solution,  it  must  be 
guaranteed  that  the  initial  orientation  error  is  less  than  9()deg,  well  within  the  accuracy  of  dead  reckoning. 


4.0  Application 


In  this  section,  we  describe  the  mobile  robot  and  the  sensor  used  in  this  application  as  well  as  the  implementation  and 
results. 
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FIGURE  1 0  .  The  Locomotion  Emulator 


4.1  The  Locomotion  Emulator 

The  Locomotion  Emulator  (LE)  is  a  mobile  robot  that  was  developed  at  the  CMU  Field  Robotics  Center  (FRC)  as  a 
testbed  for  development  of  mobile  robotic  systems  (Figure  10).  It  is  a  powerful  all-wheel  steer,  all-wheel  drive  base 
with  a  rotating  payload  platform.  A  more  complete  description  can  be  found  in  Fitzpatrick  and  Ladd  [3]. 

4.2  Cyclone 

The  Cyclone  laser  range  scanner  (Figure  11)  was  also  developed  at  the  CMU  Field  Robotics  Center  to  acquire  fast, 
precise  scans  of  range  data  over  long  distances  (up  to  50m).  ilie  sensor,  developed  by  Radartechnik  &  Elektrooptik 
[7],  consists  of  a  pulsed  Gallium  Arsenide  infrared  laser  transmitter/receiver  pair,  aimed  vertically  upward.  A  mirror 
in  the  upper  part  of  the  scanner  rotates  about  the  vertical  axis  and  deflects  the  laser  beam  so  that  it  emerges  parallel  to 
the  ground,  aeating  a  two  dimensional  ms^  of  360  degrees  field  of  view.  The  resolution  of  the  range  measurements  is 
set  to  be  10cm  and  the  accuracy  is  T20cm.  The  angular  resolution  depends  upon  the  resolution  of  the  encoder  that  is 
used  on  the  tower  motor  which  is  currently  programmed  to  acquire  1000  range  readings  per  revolution  [6]. 

It  has  been  our  experience  that  the  effects  of  the  truncation  due  to  the  resolution  of  the  scanner  and  the  non-linear 
behavior  of  the  range  measurement  along  different  operating  target  distances  must  be  considered  to  obtain  accurate 
results  in  any  position  estimation  algorithm  [15].  Rather  than  using  the  standard  10cm  range  resolution  and  +20cm 
accuracy  numbers  mentioned  above,  we  considered  the  Cyclone  scanner’s  range  offset*,  truncation  and  angular  accu¬ 
racy  characterization  obtained  in  a  separate  study  [15]. 


1 .  The  range  offset  is  a  function  which  describes  the  difference  between  the  measured  range  value  and  the  true  distance  to  the  tar¬ 
get. 
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FIGURE  11  .The  Cyclone  laser  rangefinder 


4.3  Experimental  results 

The  iconic  position  estimation  algorithm  presented  in  this  paper  was  tested  at  the  highbay  area  of  the  FRC.  Figure  12 
shows  the  line  segment  model  that  was  used  as  world  m^  in  this  tqjplicadon.  The  corridor  is  about  6m  wide  and  20m 
long.  The  solid  line  segments  denote  walls  which  were  constructed  from  wood  partitions.  We  picked  this  configura¬ 
tion  because  iu  simplicity  and  reliability  in  being  surveyed.  The  dotted  line  represents  the  path  that  the  LE  was 
instructed  to  follow.  It  consists  of  a  symmetrical  trajectory  19m  long.  The  LE,  initially  positioned  at  the  beginning  of 
the  path,  was  moved  by  steps  of  Im.  At  each  of  these  positions,  the  position  estimator  was  executed  and  the  robot 
pose  was  surveyed  using  a  theodohte.  Figure  13  shows  the  sensed  data  taken  by  the  Cyclone  at  the  7th  step.  Notice 
that  a  considerable  number  of  points  from  the  scanner  corresponds  to  objects  that  are  not  included  in  the  model  of 
Figure  12. 

The  estimator  was  programed  to  use  two  different  representations  of  the  model.  In  the  first  one,  the  model  was  repre¬ 
sented  by  8  long  line  segments  shown  in  Figure  12.  In  the  second,  each  of  these  line  segments  was  split  into  a  number 
of  small  segments  10cm  long,  providing  a  model  with  almost  400  line  segments.  The  parameter  values  used  were:  6^ 
=  5cm  and  =  5deg  for  the  LE  (5%  of  the  step  size)  and  Sj  =  10cm  and  Cj  =  0.7deg  for  the  Cyclone.  The  grid  size  was 
0.6x0.6m^. 

As  expected  the  computed  error  (surveyed  minus  estimate)  for  the  two  representations  was  the  same  at  the  20  posi¬ 
tions  along  the  path  (Figure  14).  The  maximum  position  error  was  3.6cm,  and  the  average  position  error  was  1.99cm. 
The  maximum  heading  error  was  l.Sdeg  and  the  average  was  0.73deg.  These  results  are  significant  given  the  resolu¬ 
tion  (10cm)  and  accuracy  (20cm)  of  the  scanner. 

Another  important  result  is  the  run  times.  The  estimator  was  run  on  a  Sun  Sparc  Station  1  with  a  math  coprocessor. 
For  the  8  line  segment  representation  the  approximate  run  times  were  0.37sec  for  the  preprocessing  (computation  of 
the  cdl map),  0.27sec  for  the  minimization  and  1.76  for  the  segment  correspondence,  giving  a  total  cycle  time  of  2sec. 
For  the  400  line  segment  representation,  run  times  were  12.9sec  for  the  peprocessing,  0.29sec  for  the  minimization 
and  3.22  for  the  segment  correspondence,  giving  3.5sec  of  total  c^le  time.  Note  that  by  multiplying  the  number  of 
line  segments  by  a  factor  of  50,  the  preprocessing  time  increases  considerably,  however  the  matching  time  is 
increased  only  by  a  factor  of  1.75. 

In  the  event  that  the  uncertainty  regions  for  the  sensed  points  can  be  approximated  by  circles  centered  on  the  points, 
the  segment  correspondence  can  be  computed  rapidly  using  a  numerical  Voronoi  diagram.  This  approximation 
worked  well  for  our  highbay  experiments  [19]. 
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FIGURE  14  .  Computed  errors  for  the  20  positions  along  the  path 


5.0  Conclusions 


In  this  paper  a  two  dimensional  iconic  based  approach  for  position  estimation  is  presented.  By  considering  two  reso¬ 
lution  levels  in  the  map,  a  two-stage  method  is  proposed  to  solve  the  point/line-segment  correspondence.  Further¬ 
more,  the  uncertainty  due  to  errors  in  both  dead  reckoning  pose  and  sensed  data  are  considered  in  order  to  bound  the 
searching  area.  This  approach  drastically  reduces  the  computation  time  when  the  map  is  given  by  a  high  number  of 
line  segments  (e.g.  map  built  by  the  robot  itself).  This  algorithm  was  implemented  and  tested  using  a  2-D  radial  laser 
scanner  mounted  on  a  omnidirectional  robot,  showing  for  the  first  time  an  explicit  quantification  of  the  accuracy  of  an 
iconic  position  estimator.  The  estimator  has  shown  to  be  robust  to  incompleteness  of  the  model  and  spurious  data,  and 
provides  a  highly  accurate  estimate  of  the  robot  position  and  orientation  for  many-line  environments. 
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Future  work  includes  the  comparison  of  this  method  with  a  feature-based  position  estimation  method  by  considering 
different  metrics  as  accuracy,  robustness,  starting  point  dependency,  etc.  and  the  development  of  a  more  complete 
navigation  system  including  map  building  capability  and  a  Gaussian  uncertainty  model  for  both  the  sensor  and  the 
robot  motion. 
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